/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

// Define DEBUG_OUTPUT to enable PRINTLN_DEBUG output when not using ROS.
// ROS debug level output is toggled at runtime using rxconsole.
//#define DEBUG_OUTPUT

// ================================================================== includes

#include <iostream>

using namespace std;

#include <osl_core/GeneralMath.h>

#include "nav_sim.h"

// =================================================================== defines


// =================================================================== methods
// ---------------------------------------------------------------------------
bool NavSim::init(int argc, char * argv[])
{
	useROSInput_ = true;

	tfMapFrame_ = "/map";
	tfOdomFrame_ = "odom";
	tfBaseLinkFrame_ = "base_link";
	tfGPSFrame_ = "gps";
	tfLBLFrame_ = "lbl";

	// Force request topic is non-SOA, as aardvark is not ROS enabled for now.
	rosVehicleForceReqSub_ = m_rosHandle->subscribe<geometry_msgs::WrenchStamped>("pilot/force_req", 10, &NavSim::processROSVehicleForceReq, this, m_rosHints);

 	// Published topics (providing a SOA service).
	rosNavStsPub_ = m_rosHandle->advertise<auv_msgs::NavSts>("nav/nav_sts", 10);
	rosOdometryPub_ = m_rosHandle->advertise<nav_msgs::Odometry>("nav/odometry", 10);

	if (m_useSoa)
	{
		// Register SOA services this node provides.

		// We can use one status callback for both services in this case.
		rosNavStsAd_ = m_soaClient->advertiseService("NavSts", rosNavStsPub_);
		rosOdometryAd_ = m_soaClient->advertiseService("Odometry", rosOdometryPub_);

 		// Update the status of the services for the first time.
 		updateSoaStatus();
	}


	diagPeriod_ = 1000;
	diagTimer_.setTimer(1);

	// Velocity is valid after we receive a force msg.
	velocityValid_ = false;
	forceMsgDeltaTimeLimit_ = 5.0;

	noInputTimeout_ = 1000;
	// Set no input timer to use wall time so we don't see spurious timeouts
	// when running very fast simulations (e.g. 32x).
	noInputTimer_.setUseWallTime(true);
	noInputTimer_.setTimer(noInputTimeout_);

	// TODO: Model mass and acceleration of vehicle.

	// Factors to naively convert force (+/- 1) to velocity (m/s or deg/s)
	// The very simple model used here assumes terminal velocity for a given force is reached
	// instantaneously. So inertia and acceleration are ignored.

	xForceToVelocity_ = 1.0;
	yForceToVelocity_ = 0.5;
	zForceToVelocity_ = 0.5;
	rollForceToVelocity_ = 0;
	pitchForceToVelocity_ = 45.0;
	yawForceToVelocity_ = 45.0;

	//Reading from ROS param
	
	double originLat;
	double originLon;
	if (!m_conf.getDouble(originLat, "origin_latitude", "NAV_SIM")) {
		originLat = 56.380033;
		PRINTLN_INFO("No origin_latitude parameter; using default: " << originLat);
	}
	else {
		PRINTLN_INFO("Origin latitude : " << originLat);
	}

	if (!m_conf.getDouble(originLon, "origin_longitude", "NAV_SIM")) {
		originLon = -4.271616;
		PRINTLN_INFO("No origin_latitude parameter; using default: " << originLon);
	}
	else {
		PRINTLN_INFO("Origin longitude: " << originLon);
	}

	// Set global origin.
	globalOrigin_.lat = (double)originLat;
	globalOrigin_.lon = (double)originLon;
	globalOrigin_.depth = 0;
	worldToGlobal_.setLLDReference(globalOrigin_);

	// Default to starting at the origin, with zero roll, pitch and yaw.
	worldPosition_.n = 0;
	worldPosition_.e = 0;
	worldPosition_.d = 0;
	worldOrientation_.r = 0;
	worldOrientation_.p = 0;
	worldOrientation_.y = 0;

	// Try to get start NED position and yaw, but fallback on defaults.
	float tempFloat;
	if (m_conf.getFloat(tempFloat, "start_north", "NAV_SIM")) worldPosition_.n = tempFloat;
	if (m_conf.getFloat(tempFloat, "start_east", "NAV_SIM")) worldPosition_.e = tempFloat;
	if (m_conf.getFloat(tempFloat, "start_depth", "NAV_SIM")) worldPosition_.d = tempFloat;
	if (m_conf.getFloat(tempFloat, "start_pitch_deg", "NAV_SIM")) worldOrientation_.y = tempFloat;
	if (m_conf.getFloat(tempFloat, "start_yaw_deg", "NAV_SIM")) worldOrientation_.y = tempFloat;

	// Check if we take altitude
	usingUWSimAltitude_=false;
	if (m_conf.getBool(usingUWSimAltitude_, "using_UWSim_altitude", "NAV_SIM") && usingUWSimAltitude_) {
		PRINTLN_INFO("Subscibing to topic UWSim/altitude for altitude.");
	
		rosUWSimAltitudeSub_ = m_rosHandle->subscribe<sensor_msgs::Range>("UWSim/altitude", 10, &NavSim::processUWSimAltitude, this);
		//Initialization of the altitude
		altitude_=0;
	}
	else if(m_conf.getFloat(waterColumnDepth_, "waterColumnDepth", "NAV_SIM")){
		PRINTLN_INFO("Using waterColumnDepth defined by user: " << waterColumnDepth_);
		altitude_ = waterColumnDepth_ - worldPosition_.d;
	}
	else {
		waterColumnDepth_ = 10;
		PRINTLN_INFO("Using default waterColumnDepth " << waterColumnDepth_);
			altitude_ = waterColumnDepth_ - worldPosition_.d;
	}

	totalTimeElapsed_=0;

	//Parameters for the simulation of the current and buoyancy.
	enableCurrentSim_ = false;
	periodWaves_ = 6.0;
	currentDirection_ = 0.0;
	maxVelocity_ = 0.05;
	buoyantVelocity_ = 0.1;
	m_conf.getBool(enableCurrentSim_, "enable_current_sim", "NAV_SIM"); // arg untouched if param not specified
	if (m_conf.getFloat(tempFloat, "period_waves", "NAV_SIM")) periodWaves_ = tempFloat;
	if (m_conf.getFloat(tempFloat, "current_direction", "NAV_SIM")) currentDirection_ = tempFloat;
	if (m_conf.getFloat(tempFloat, "max_velocity_current", "NAV_SIM")) maxVelocity_ = tempFloat;
	if (m_conf.getFloat(tempFloat, "buoyant_velocity", "NAV_SIM")) buoyantVelocity_ = tempFloat;

	noiseScale_=0.0;
	if (m_conf.getFloat(tempFloat, "noise_scale", "NAV_SIM")) noiseScale_ = tempFloat;


	PRINTLN_INFO("Start position: north " << worldPosition_.n << ", east " << worldPosition_.e
			<< ", depth " << worldPosition_.d << ", altitude " << altitude_);
	PRINTLN_INFO("Start orientation: roll " << worldOrientation_.r
			<< ", pitch " << worldOrientation_.p << ", yaw " << worldOrientation_.y);

	if (!m_conf.getFloat(maxWallTimeSendFreq_, "maxWallTimeSendFreq", "NAV_SIM")) {
		maxWallTimeSendFreq_ = 25;
		PRINTLN_INFO("Using default maxWallTimeSendFreq " << maxWallTimeSendFreq_);
	}

	if (maxWallTimeSendFreq_ == 0) {
		minWallTimeSendPeriod_ = 1;
	}
	else {
		minWallTimeSendPeriod_ = 1000 / maxWallTimeSendFreq_ + 0.5;
	}
	PRINTLN_INFO("maxWallTimeSendFreq " << maxWallTimeSendFreq_ << " Hz -> " << minWallTimeSendPeriod_ << " ms period.");
	// Set send timer to use wall time, and expire straight away initially.
	wallTimeSendTimer_.setUseWallTime(true);
	wallTimeSendTimer_.setTimer(1);

	PRINTLN_INFO("nav sim started - will produce nav sts msg for each force msg received.");

	return true;
}


// ---------------------------------------------------------------------------
void NavSim::updateSoaStatus()
{
	if (rosNavStsPub_.getNumSubscribers() == 0) {
		rosNavStsAd_.setStatus(matchmaker::ServiceStatus::AVAILABLE);
	}
	else {
		rosNavStsAd_.setStatus(matchmaker::ServiceStatus::ACTIVE);
	}

	if (rosOdometryPub_.getNumSubscribers() == 0) {
		rosOdometryAd_.setStatus(matchmaker::ServiceStatus::AVAILABLE);
	}
	else {
		rosOdometryAd_.setStatus(matchmaker::ServiceStatus::ACTIVE);
	}

//	ROS_INFO_STREAM_THROTTLE(1.0, "NavSts subscribers " << rosNavStsPub_.getNumSubscribers()
//			<< ", Odometry subscribers " << rosOdometryPub_.getNumSubscribers());
}

// ---------------------------------------------------------------------------

void NavSim::processUWSimAltitude(const sensor_msgs::RangeConstPtr & msg)
{
	altitude_=msg->range;
}

// ---------------------------------------------------------------------------

void NavSim::processROSVehicleForceReq(const geometry_msgs::WrenchStampedConstPtr & msg)
{
	if (!useROSInput_) return;

	noInputTimer_.setTimer(noInputTimeout_);

	if (velocityValid_) {
		ros::Duration elapsedRosTime = msg->header.stamp - lastROSForceTime_;
		double elapsedTime = elapsedRosTime.toSec();
		updatePositionFromVelocity(elapsedTime);
	}

	setVelocityFromForce(msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z,
			msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z);

	lastROSForceTime_ = msg->header.stamp;
	velocityValid_ = true;

	// Always send ROS NavSts to pilot at full rate.
	sendROSNavSts();

	// Limit rate of publishing ROS Odometry, TF messages, and OceanSHELL.
	if (wallTimeSendTimer_.getTimer() == 0) {
		wallTimeSendTimer_.incTimer(minWallTimeSendPeriod_);

		sendROSOdometry();

	}
	else {
		//PRINTLN_INFO("Rate limiting message sending, min wall time send period " << minWallTimeSendPeriod_);
	}
}

// ---------------------------------------------------------------------------
void NavSim::sendROSNavSts()
{
	rosNavStsMsg_.header.stamp = lastROSForceTime_;
	rosNavStsMsg_.origin.latitude = globalOrigin_.lat;
	rosNavStsMsg_.origin.longitude = globalOrigin_.lon;

	rosNavStsMsg_.global_position.latitude = globalPosition_.lat;
	rosNavStsMsg_.global_position.longitude = globalPosition_.lon;

	rosNavStsMsg_.position.north = worldPosition_.n;
	rosNavStsMsg_.position.east = worldPosition_.e;
	rosNavStsMsg_.position.depth = worldPosition_.d;
	rosNavStsMsg_.altitude = altitude_;

	// Convert to radians for ROS message.
	rosNavStsMsg_.orientation.roll = osl_core::deg2rad(worldOrientation_.r);
	rosNavStsMsg_.orientation.pitch = osl_core::deg2rad(worldOrientation_.p);
	rosNavStsMsg_.orientation.yaw = osl_core::deg2rad(worldOrientation_.y);

	// Here we lie about the velocities. We report them to be a factor smaller
	// than the ones we are applying to the vehicle model, as a fudge to give
	// more stable control from the pilot.
	const float velocityScale = 0.2;
	rosNavStsMsg_.body_velocity.x = bodyPositionVelocity_.u * velocityScale;
	rosNavStsMsg_.body_velocity.y = bodyPositionVelocity_.v * velocityScale;
	rosNavStsMsg_.body_velocity.z = bodyPositionVelocity_.w * velocityScale;

	// Convert to radians for ROS message.
	rosNavStsMsg_.orientation_rate.roll = osl_core::deg2rad(bodyOrientationVelocity_.r * velocityScale);
	rosNavStsMsg_.orientation_rate.pitch = osl_core::deg2rad(bodyOrientationVelocity_.p * velocityScale);
	rosNavStsMsg_.orientation_rate.yaw = osl_core::deg2rad(bodyOrientationVelocity_.y * velocityScale);

	//rosNavStsMsg_.mode = auv_msgs::NavSts::MODE_DEFAULT;
	rosNavStsMsg_.status = auv_msgs::NavSts::STATUS_ALL_OK;

	rosNavStsPub_.publish(rosNavStsMsg_);
}

// ---------------------------------------------------------------------------
void NavSim::sendROSOdometry()
{
	// This message isn't needed by the AUV, but allows us to use ROS rviz to
	// visualise the vehicle position.

	// Map from AUV to ROS coordinates.
	double rosX, rosY, rosZ, rosRoll, rosPitch, rosYaw;
	osl_core::mapAxisAUVtoROS(worldPosition_.n, worldPosition_.e, worldPosition_.d, rosX, rosY, rosZ);
	osl_core::mapAxisAUVtoROS(
			osl_core::deg2rad(worldOrientation_.r),
			osl_core::deg2rad(worldOrientation_.p),
			osl_core::deg2rad(worldOrientation_.y),
			rosRoll, rosPitch, rosYaw);
	geometry_msgs::Quaternion rosOrientation = tf::createQuaternionMsgFromRollPitchYaw(rosRoll, rosPitch, rosYaw);

	rosOdometryMsg_.header.stamp = lastROSForceTime_;
	// TODO: Start sending odom as well as map transforms
	rosOdometryMsg_.header.frame_id = tfMapFrame_;
	rosOdometryMsg_.child_frame_id = tfBaseLinkFrame_;
	rosOdometryMsg_.pose.pose.position.x = rosX;
	rosOdometryMsg_.pose.pose.position.y = rosY;
	rosOdometryMsg_.pose.pose.position.z = rosZ;
	rosOdometryMsg_.pose.pose.orientation = rosOrientation;

	rosOdometryPub_.publish(rosOdometryMsg_);

	// Create and publish Transform msgs.
	// Broadcaster needs to be static not member variable as it cannot be created before
	// ModuleCore has initialised ROS.
	static tf::TransformBroadcaster rosTFBroadcaster;
	tf::Transform transform;
	// First map -> base_link
	transform.setOrigin(tf::Vector3(rosX, rosY, rosZ));
	transform.setRotation(tf::createQuaternionFromRPY(rosRoll, rosPitch, rosYaw));
	rosTFBroadcaster.sendTransform(tf::StampedTransform(transform, rosOdometryMsg_.header.stamp,
			tfMapFrame_, tfBaseLinkFrame_));
	// Then base_link -> map
	transform.setOrigin(tf::Vector3(-rosX, -rosY, -rosZ));
	transform.setRotation(tf::createQuaternionFromRPY(-rosRoll, -rosPitch, -rosYaw));
	rosTFBroadcaster.sendTransform(tf::StampedTransform(transform, rosOdometryMsg_.header.stamp,
			(tfMapFrame_ + "/" + tfBaseLinkFrame_), tfMapFrame_));
}



// ---------------------------------------------------------------------------
void NavSim::doWork()
{
	if (noInputTimer_.getTimer() == 0 && diagTimer_.getTimer() == 0) {
		diagTimer_.setTimer(5000);
		PRINTLN_WARN("No force input; nav output disabled.");
	}

	if (m_useSoa) updateSoaStatus();
}

// ---------------------------------------------------------------------------
void NavSim::setVelocityFromForce(double x, double y, double z, double roll, double pitch, double yaw)
{
	x = osl_core::limitPlusMinus(x, 1);
	y = osl_core::limitPlusMinus(y, 1);
	z = osl_core::limitPlusMinus(z, 1);
	roll = osl_core::limitPlusMinus(roll, 1);
	pitch = osl_core::limitPlusMinus(pitch, 1);
	yaw = osl_core::limitPlusMinus(yaw, 1);

	bodyPositionVelocity_.u = x * xForceToVelocity_;
	bodyPositionVelocity_.v = y * yForceToVelocity_;
	bodyPositionVelocity_.w = z * zForceToVelocity_;
	bodyOrientationVelocity_.r = roll * rollForceToVelocity_;
	bodyOrientationVelocity_.p = pitch * pitchForceToVelocity_;
	bodyOrientationVelocity_.y = yaw * yawForceToVelocity_;

	// Simulate pitch rotation force that will bring the vehicle back to zero pitch.
	// Just use the square root of pitch angle, in degrees - not physically accurate,
	// but looks quite realistic.
	if (worldOrientation_.p > 0) {
		bodyOrientationVelocity_.p -= sqrt(fabs(worldOrientation_.p));
	}
	else if (worldOrientation_.p < 0) {
		bodyOrientationVelocity_.p += sqrt(fabs(worldOrientation_.p));
	}
}

// ---------------------------------------------------------------------------
void NavSim::updatePositionFromVelocity(double elapsedTime)
{
	if (!velocityValid_) {
		PRINTLN_ERR("updatePositionFromVelocity called when velocity is not valid!");
		return;
	}

	if (elapsedTime <= 0)
	{
		// Zero elapsed time is quite common when scaling simulation time, so don't print anything.
		return;
	}
	else if (elapsedTime > forceMsgDeltaTimeLimit_)
	{
		PRINTLN_WARN("Elapsed time between msgs over limit: " << elapsedTime << " : ignoring previous force.");
		return;
	}

	
	// Here we apply the vehicle velocity (calculated from the last force)

	osl_core::T_VCS bodyTranslation;
	osl_core::T_RPY bodyRotation;

	bodyTranslation.u = bodyPositionVelocity_.u * elapsedTime;
	bodyTranslation.v = bodyPositionVelocity_.v * elapsedTime;
	bodyTranslation.w = bodyPositionVelocity_.w * elapsedTime;

	bodyRotation.r = bodyOrientationVelocity_.r * elapsedTime;
	bodyRotation.p = bodyOrientationVelocity_.p * elapsedTime;
	bodyRotation.y = bodyOrientationVelocity_.y * elapsedTime;

	bodyToWorld_.setReferenceDeg(worldPosition_, worldOrientation_);
	// New world position - applying translation before rotation.
	worldPosition_ = bodyToWorld_.VCS2NED(bodyTranslation);

	// Current simulation
	if (enableCurrentSim_){

		//Compute the current velocity based in a sinusuidal model.

		totalTimeElapsed_=totalTimeElapsed_+elapsedTime;
		
		float CurrentVelocity;
		CurrentVelocity = sin((totalTimeElapsed_-floor(totalTimeElapsed_/periodWaves_)*periodWaves_)/periodWaves_*M_PI*2) * maxVelocity_/2 + maxVelocity_/2;

		worldPosition_.n = worldPosition_.n + CurrentVelocity* cos (osl_core::deg2rad(currentDirection_))*elapsedTime;
		worldPosition_.e= worldPosition_.e + CurrentVelocity* sin (osl_core::deg2rad(currentDirection_))*elapsedTime;
	
		// We approximate buoyant force to velocity to simplify.

		worldPosition_.d= worldPosition_.d - buoyantVelocity_*elapsedTime;
	}	

	// We add some gaussian noise in NED yaw and pitch
	worldPosition_.n = worldPosition_.n + osl_core::GetRandomDoubleUsingNormalDistribution(0,noiseScale_/40) * elapsedTime;
	worldPosition_.e = worldPosition_.e + osl_core::GetRandomDoubleUsingNormalDistribution(0,noiseScale_/40) * elapsedTime;
	worldPosition_.d = worldPosition_.d + osl_core::GetRandomDoubleUsingNormalDistribution(0,noiseScale_/40) * elapsedTime;
	bodyRotation.y = bodyRotation.y + osl_core::GetRandomDoubleUsingNormalDistribution(0,noiseScale_) * elapsedTime;
	bodyRotation.p = bodyRotation.p + osl_core::GetRandomDoubleUsingNormalDistribution(0,noiseScale_) * elapsedTime;


	// Check for minimum depth.
	if (worldPosition_.d < 0) {
		// Clamp depth to zero.
		worldPosition_.d = 0;
	}

	if(!usingUWSimAltitude_){
		// Check depth max
		if (worldPosition_.d > waterColumnDepth_) {
			// Clamp depth to max
			worldPosition_.d = waterColumnDepth_;
		}
		//Update altitude
		altitude_ = waterColumnDepth_ - worldPosition_.d;
	}
	else {
		if(altitude_<=0){
			// TODO: Keep the vehicle at same depth...
		} 
	}

	// TODO: Deal properly orientations wrapping round, i.e. wrapping pitch over 90 forces
	// change in roll and yaw, and pitch then starts decreasing.

	// New world orientation. Don't allow roll or pitch over 80 degrees, for simplicity.
	worldOrientation_.r = osl_core::limitPlusMinus(worldOrientation_.r + bodyRotation.r, 80);
	worldOrientation_.p = osl_core::limitPlusMinus(worldOrientation_.p + bodyRotation.p, 80);
	worldOrientation_.y = osl_core::wrapPlusMinus180(worldOrientation_.y + bodyRotation.y);

	globalPosition_ = worldToGlobal_.NED2LLD(worldPosition_);
	globalPosition_.depth = worldPosition_.d;

//	PRINTLN_DEBUG("Body pose change x " << fixed << setprecision(3) << bodyTranslation.u << ", y " << bodyTranslation.v << ", z " << bodyTranslation.w
//			<< setprecision(1) << ", roll " << bodyRotation.r << ", pitch " << bodyRotation.p << ", yaw " << bodyRotation.y);
//	PRINTLN_DEBUG("New world pose n " << fixed << setprecision(3) << worldPosition_.n << ", e " << worldPosition_.e << ", d " << worldPosition_.d << ", a " << altitude_
//			<< setprecision(1) << ", roll " << worldOrientation_.r << ", pitch " << worldOrientation_.p << ", yaw " << worldOrientation_.y);

	if (diagTimer_.getTimer() == 0)
	{
		diagTimer_.setTimer(diagPeriod_);
		PRINTLN_DEBUG("Position: n " << fixed << setprecision(3) << worldPosition_.n << ", e " << worldPosition_.e << ", d " << worldPosition_.d << ", a " << altitude_);
		PRINTLN_DEBUG("Orientation: roll " << fixed << setprecision(1) << worldOrientation_.r << ", pitch " << worldOrientation_.p << ", yaw " << worldOrientation_.y);
	}
}

// ---------------------------------------------------------------------------
void NavSim::cleanup()
{
	PRINTLN_DEBUG("cleanup entered");
}

// ===========================================================================
int main (int argc, char * argv[])
{
	NavSim modCore;
	exit(modCore.main(argc, argv, true /* noWatcher */));
}
